﻿/*****************************************************************//**
 * \file   AuboRobotControl.cpp
 * \brief  奥博机械臂控制相关代码
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/

#include "AuboRobotControl.h"
#include <iostream>

#include <fstream>

#include <sys/time.h>
#include <unistd.h>
#include <stdio.h>
using namespace std;

fstream fsMoveFollow;
bool openfile = false;

/**
 * \brief   AuboRobotControl类构造函数.
 *
 */
AuboRobotControl::AuboRobotControl()
{
    bool bFollowStart;
    QFile fsMoveFollow;
    bool openfile = false;
    std::chrono::high_resolution_clock::time_point startTime;
    std::chrono::high_resolution_clock::time_point endTime;
    bFollowStart = false;

    // 使用 std::chrono 替代 QueryPerformanceFrequency
    startTime = std::chrono::high_resolution_clock::now();

    if (!openfile) {
        fsMoveFollow.setFileName("/home/user/experiment_data/fsMoveFollow.txt"); // 替换为 Linux 文件路径
        if (fsMoveFollow.open(QIODevice::WriteOnly | QIODevice::Text)) {
            openfile = true;
        } else {
            // 处理文件打开失败的情况
            qWarning("Failed to open file for writing.");
        }
    }

//	bFollowStart = false;
//	QueryPerformanceFrequency(&liQpfrequency);
//	if (openfile == false) {
//		fsMoveFollow.open("C:\\20210812\\SurgicalRobotPlatform\\SurgicalRobotPlatform\\experiment_data\\fsMoveFollow.txt", std::ios::out);
//		openfile = true;
//	}

}

/**
 * \brief   AuboRobotControl析构函数.
 *
 */
AuboRobotControl::~AuboRobotControl()
{
	fsMoveFollow.close();
}

/**
 * \brief   机械臂初始化与登陆机械臂.
 *
 * \param addr 机械臂服务器地址
 * \param port 机械臂服务器端口
 * \return  true 成功 false 失败
 */

#define SERVER_HOST "192.168.65.140"
#define SERVER_PORT 8899
bool AuboRobotControl::robotLogin(const char* addr, int port)
{
    //linux
    int ret = aubo_robot_namespace::InterfaceCallSuccCode;
    /** Interface call: login ***/
    ret = robotService.robotServiceLogin(SERVER_HOST, SERVER_PORT, "aubo", "123456");
    if(ret == aubo_robot_namespace::InterfaceCallSuccCode)
    {
        std::cout<<"login successful."<<std::endl;
    }
    else
    {
        std::cerr<<"login failed."<<std::endl;
    }
}

/**
 * \brief   启动机械臂(必须连接真实机械臂）.
 *
 * \return  true 成功 false 失败
 */
bool AuboRobotControl::robotStartup()
{
    //linux
    /** If the real robot arm is connected, the arm needs to be initialized.　**/
    aubo_robot_namespace::ROBOT_SERVICE_STATE result;

    //Tool dynamics parameter
    aubo_robot_namespace::ToolDynamicsParam toolDynamicsParam;
    memset(&toolDynamicsParam, 0, sizeof(toolDynamicsParam));

    int ret = robotService.rootServiceRobotStartup(toolDynamicsParam/**Tool dynamics parameter**/,
                                                   6        /*Collision level*/,
                                                   true     /*Whether to allow reading poses defaults to true*/,
                                                   true,    /*Leave the default to true */
                                                   1000,    /*Leave the default to 1000 */
                                                   result); /*Robot arm initialization*/
    if(ret == aubo_robot_namespace::InterfaceCallSuccCode)
    {
        std::cout<<"Robot arm initialization succeeded."<<std::endl;
    }
    else
    {
        std::cerr<<"Robot arm initialization failed."<<std::endl;
    }

    return result;
}


/**
 * \brief   关闭机械臂（必须连接真实机械臂）.
 *
 * \return  true 成功 false 失败
 */
bool AuboRobotControl::robotShutdown()
{
    //linux
    robotService.robotServiceLeaveTcp2CanbusMode();
    bFollowStart = false;
    return robotService.robotMoveFastStop() == aubo_robot_namespace::InterfaceCallSuccCode ? true : false;
}




/**
 * \brief          机械臂关节运动.
 *
 * \param initPos  机械臂运动到的位置
 * \return         true 成功 false 失败
 */
bool AuboRobotControl::robotMoveJ(double* initPos)
{
	bool result = false;
    //linux
    aubo_robot_namespace::RobotRecongnitionParam param;
    aubo_robot_namespace::JointVelcAccParam joint_maxacc = { 0 };
    aubo_robot_namespace::JointVelcAccParam joint_maxvel = { 0 };

	for (int i = 0; i < 6; i++)
	{
		joint_maxacc.jointPara[i] = 0.5;
		joint_maxvel.jointPara[i] = 0.5;
	}

    //linux
    robotService.robotServiceLeaveTcp2CanbusMode();
    robotService.robotServiceSetGlobalMoveJointMaxAcc(joint_maxacc);
    robotService.robotServiceSetGlobalMoveJointMaxVelc(joint_maxvel);
    robotService.robotServiceGetRobotRecognitionParam(0,param);

    double jointAngle[aubo_robot_namespace::ARM_DOF];
    for(int i=0;i<aubo_robot_namespace::ARM_DOF;i++)
    {
        jointAngle[i]=initPos[i];
    }
    if (robotService.robotServiceJointMove(jointAngle, true) == aubo_robot_namespace::InterfaceCallSuccCode)
    {
        result = true;
        std::cout << "movej succ" << std::endl;
    }
    else
    {
        std::cerr << "movej failed!" << std::endl;
    }

    return result;
}

/**
 * \brief   机械臂运动到目标位置.
 *
 * \param target_postion 目标位置
 */
void AuboRobotControl::robotMove(double* target_postion)
{
	robotMoveFollow(target_postion);

	return;
}

MyStruct* AuboRobotControl::Polynomia5_interpolation(Eigen::VectorXd Lastpoint, Eigen::VectorXd Newpoint, Eigen::VectorXd Lastpointv, Eigen::VectorXd Newpointv, Eigen::VectorXd  Lastpointa, Eigen::VectorXd  Newpointa, double time, double N)
{
	Eigen::VectorXd a0(6), a1(6), a2(6), a3(6), a4(6), a5(6), h(6);
	//double  q_p[N][6], q_v(N)(6), q_a(N)(6);
	//= (MyStruct*)malloc(sizeof(double) * N * 6 * 3);
	double T, t;
	MyStruct* sData = (MyStruct*)malloc(sizeof(double) * N * 6 * 3);


//    sData->q_p = new double* [N];
//    sData->q_v = new double* [N];
//    sData->q_a = new double* [N];

    sData->q_p = new double*[static_cast<int>(N)];
    sData->q_v = new double*[static_cast<int>(N)];
    sData->q_a = new double*[static_cast<int>(N)];

	for (int j = 0; j < N; j++)
	{
		sData->q_p[j] = new double[6];
		sData->q_v[j] = new double[6];
		sData->q_a[j] = new double[6];
	}

	T = time;
	for (int j = 0; j < 6; j++)
	{
		h[j] = Newpoint[j] - Lastpoint[j];
		a0[j] = Lastpoint[j];
		a1[j] = Lastpointv[j];
		a2[j] = Lastpointa[j] / 2;
		a3[j] = (20 * h[j] - (8 * Newpointv[j] + 12 * Lastpointv[j]) * T - (3 * Lastpointa[j] - Newpointa[j]) * T * T) / (2 * T * T * T);
		a4[j] = (-30 * h[j] + (14 * Newpointv[j] + 16 * Lastpointv[j]) * T + (3 * Lastpointa[j] - 2 * Newpointa[j]) * T * T) / (2 * T * T * T * T);
		a5[j] = (12 * h[j] - 6 * (Newpointv[j] + Lastpointv[j]) * T - (Lastpointa[j] - Newpointa[j]) * T * T) / (2 * T * T * T * T * T);
	}

	t = time / N;
	for (int i = 0; i < N; i++)
	{
		t = t + time / N;
		for (int j = 0; j < 6; j++)
		{
			sData->q_p[i][j] = a0[j] + a1[j] * t + a2[j] * t * t + a3[j] * t * t * t + a4[j] * t * t * t * t + a5[j] * t * t * t * t * t;
			sData->q_v[i][j] = a1[j] + 2 * a2[j] * t + 3 * a3[j] * t * t + 4 * a4[j] * t * t * t + 5 * a5[j] * t * t * t * t;
			sData->q_a[i][j] = 2 * a2[j] + 6 * a3[j] * t + 12 * a4[j] * t * t + 20 * a5[j] * t * t * t;
		}
	}
	//sData.q_p = q_p;
	//sData.q_v = q_v;
	//sData.q_a = q_a;

	return sData;
	//return q_p;
}

/**
 * \brief   机械臂运动到目标位置.
 *
 * \param initPos  目标位置
 * \return          true表示成功，false便是失败
 */
bool AuboRobotControl::robotMoveFollowOld(double* initPos)
{
//	MyStruct* sData = nullptr;

//	//计算速度专用
//	if (bFollowStart)
//	{
//		QueryPerformanceCounter(&liEndtime);
//		double dSamplet;
//		dSamplet = ((double)(liEndtime.QuadPart - lilasttime.QuadPart)) / ((double)(liQpfrequency.QuadPart));

//		//fsMoveFollow << dSamplet << "\n";

//		lilasttime = liEndtime;
//		//std::ofstream out("C:/print.txt",ios::app);
//		rs_get_diagnosis_info(rshdHand, &ala);

//		int sendtime = (int)(1 + dSamplet / 0.0049);
//		if (sendtime > 96)
//		{
//			return false;
//		}
//		//int sendtime = (int)(1 + dSamplet / 0.0049);
//		Eigen::VectorXd ljoint(6), njoint(6), deta(6), ljointv(6), ljointa(6), lastk(6), njointv(6), njointa(6);
//		//ljoint,njoint;
//		for (int i = 0; i < 6; i++)
//		{
//			ljoint[i] = dLastJoint[i];
//			njoint[i] = initPos[i];
//			ljointv[i] = dLastJointV[i];
//			ljointa[i] = dLastJointA[i];
//			//njoint[i] = last_joint[i]+0.003;
//		}


//		for (int i = 0; i < 6; i++) {
//			njointv[i] = ((njoint[i] - ljoint[i]) / dSamplet) * 2 - ljointv[i];
//			njointa[i] = (njointv[i] - ljointv[i]) / dSamplet;
//		}






//		sData = Polynomia5_interpolation(ljoint, njoint, ljointv, njointv, ljointa, ljointv, dSamplet, sendtime);


//		int kk = (int)(sendtime / 4);

//		if (ala.macTargetPosDataSize < 96 && ala.macTargetPosDataSize >= 0)
//		{
//			if (ala.macTargetPosDataSize < 6)
//			{
//				for (int i = 0; i < 6; i++)
//				{
//					dLastJointV[i] = 0;
//					dLastJointA[i] = 0;
//					ljointv[i] = 0;
//					ljointa[i] = 0;
//				}
//			}
//			for (int k = 0; k < kk; k++) {
//				for (int i = 0; i < 4; i++) {
//					for (int j = 0; j < 6; j++) {
//						//tcp2canbus_4points[i][j] = ljoint[j] + ((double)i + 1) * last_jointV[i] + (double)i / 3.0 * (((double)i + 1) / 4.0 * (njoint[j] - ljoint[j]) - ((double)i + 1) * last_jointV[i]);
//						dTcp2Canbus4Points[i][j] = sData->q_p[4 * k + i][j];
//					}
//				}
//				rs_set_waypoints_to_canbus(rshdHand, dTcp2Canbus4Points, 4);
//			}



//			//out.close();
//			for (int i = 0; i < 6; i++)
//			{
//				dLastJoint[i] = sData->q_p[4 * kk][i];
//				dLastJointV[i] = sData->q_v[4 * kk][i];
//				dLastJointA[i] = sData->q_a[4 * kk][i];
//				njoint[i] = sData->q_p[4 * kk][i];
//			}

//			ljoint = njoint;
//		}

//#if 0
//		for (int i = 0; i < 6; i++)
//		{
//			delete[] sData->q_p[i];
//			sData->q_p[i] = nullptr;
//			delete[] sData->q_v[i];
//			sData->q_v[i] = nullptr;
//			delete[] sData->q_a[i];
//			sData->q_a[i] = nullptr;
//		}
//#endif

//		delete[] sData->q_p;
//		sData->q_p = nullptr;
//		delete[] sData->q_v;
//		sData->q_v = nullptr;
//		delete[] sData->q_a;
//		sData->q_a = nullptr;

//		//delete[] sData->q_p;
//		//sData->q_p = nullptr;
//		//delete[] sData->q_v;
//		//sData->q_v = nullptr;
//		//delete[] sData->q_a;
//		//sData->q_a = nullptr;

//		//free(sData);
//		sData = nullptr;
//	}
//	else
//	{
//		JointVelcAccParam joint_maxacc = { 0 };
//		JointVelcAccParam joint_maxvel = { 0 };
//		for (int i = 0; i < 6; i++) {
//			joint_maxacc.jointPara[i] = 0.5;
//			joint_maxvel.jointPara[i] = 0.5;
//		}
//		rs_set_global_joint_maxacc(rshdHand, &joint_maxacc);
//		rs_set_global_joint_maxvelc(rshdHand, &joint_maxvel);

//		//JointVelcAccParam joint_maxacc = { 0 };
//		//JointVelcAccParam joint_maxvel = { 0 };
//		JointStatus jointStatus[6] = { 0 };
//		// 		Eigen::Matrix3d robot_orimatrix;
//		// 		Eigen::Vector3d robot_transmat;
//				/*	for (int i = 0; i < 6; i++) {
//						joint_maxacc.jointPara[i] = 0.5;
//						joint_maxvel.jointPara[i] = 0.5;
//					}*/
//					/*	rs_set_global_joint_maxacc(rshd, &joint_maxacc);
//						rs_set_global_joint_maxvelc(rshd, &joint_maxvel);*/
//		rs_leave_tcp2canbus_mode(rshdHand);
//		//开始就退出透传
//		robotGetJoint(dLastJoint);
//		//robot_forward(rshd, current_joint, robot_orimatrix, robot_transmat);
//  //      robot_startMat.block(0, 0, 3, 3) = robot_orimatrix;
//  //      robot_startMat.block(0, 3, 3, 1) = robot_transmat;
//		rs_enter_tcp2canbus_mode(rshdHand);
//		//std::ofstream out("C:/print1.txt", ios::app);
//		for (int i = 0; i < 4; i++) {
//			for (int j = 0; j < 6; j++)
//			{
//				dTcp2Canbus4Points[i][j] = dLastJoint[j];

//				//out << tcp2canbus_6points[i][j] << " ";
//				if (i >= 3)
//				{
//					dLastJointV[j] = 0;
//					dLastJointA[j] = 0;
//				}

//			}
//			//out << endl;
//		}
//		//out.close();

//		rs_set_waypoints_to_canbus(rshdHand, dTcp2Canbus4Points, 4);

//		QueryPerformanceCounter(&lilasttime);

//		bFollowStart = true;
//	}

//	return false;
}


/**
 * \brief   机械臂运动到目标位置.
 *
 * \param initPos  目标位置
 * \return          true表示成功，false便是失败
 */
bool AuboRobotControl::robotMoveFollow(double* initPos)
{
    //计算速度专用
        if (bFollowStart)
        {
            //try other way
            //这里用高精度定时器模拟实时补偿微分速度？
            //QueryPerformanceCounter(&liEndtime);
            double dSamplet;
            //dSamplet = ((double)(liEndtime.QuadPart - lilasttime.QuadPart)) / ((double)(liQpfrequency.QuadPart));

            struct timespec time1, time2;
            double elapsed_time=0.0;
            clock_gettime(CLOCK_MONOTONIC,&time1);
            usleep(100000);
            clock_gettime(CLOCK_MONOTONIC,&time2);
            elapsed_time = (time2.tv_sec - time1.tv_sec) *1000+(time2.tv_nsec - time1.tv_nsec)/1000000;
            printf("Elapsed time: %lf(ms)\n",elapsed_time);

            //fsMoveFollow << dSamplet << "\n";
            lilasttime = liEndtime;
            //std::ofstream out("C:/print.txt",ios::app);

            //win
            //rs_get_diagnosis_info(rshdHand, &ala);
            //linux
            robotService.robotServiceGetRobotDiagnosisInfo(ala);
            //fsMoveFollow << "rshdHand:" << rshdHand << "\n";


            int sendtime = (int)(1 + dSamplet / 0.019);
            if (sendtime > 20)
            {
                return false;
            }
            //int sendtime = (int)(1 + dSamplet / 0.0049);
            Eigen::VectorXd ljoint(6), njoint(6), deta(6), ljointv(6), ljointa(6), lastk(6);
            //ljoint,njoint;
            for (int i = 0; i < 6; i++)
            {
                ljoint[i] = dLastJoint[i];
                njoint[i] = initPos[i];
                ljointv[i] = dLastJointV[i];
                ljointa[i] = dLastJointA[i];
                //njoint[i] = last_joint[i]+0.003;
            }
            deta = (njoint - ljoint) / sendtime;
            for (int kk = 0; kk < sendtime; kk++)
            {
                njoint = ljoint + deta;
                //out << dSamplet << " ";

                if (ala.macTargetPosDataSize < 96 && ala.macTargetPosDataSize >= 0)
                {
                    if (ala.macTargetPosDataSize < 6)
                    {
                        for (int i = 0; i < 6; i++)
                        {
                            dLastJointV[i] = 0;
                            dLastJointA[i] = 0;
                            ljointv[i] = 0;
                            ljointa[i] = 0;
                        }
                    }

                    if ((njoint - ljoint).norm() > 0.01)
                    {
                        njoint = ljoint + 0.01 / (njoint - ljoint).norm() * (njoint - ljoint);
                    }
                    lastk = njoint - ljoint - ljointv - ljointa;
                    //njoint 跳跃


                    //fsMoveFollow << kk << "\n";

                    for (int i = 0; i < 4; i++) {
                        for (int j = 0; j < 6; j++) {
                            //tcp2canbus_4points[i][j] = ljoint[j] + ((double)i + 1) * last_jointV[i] + (double)i / 3.0 * (((double)i + 1) / 4.0 * (njoint[j] - ljoint[j]) - ((double)i + 1) * last_jointV[i]);
                            dTcp2Canbus4Points[i][j] = ljoint[j] + ((double)i + 1) * 0.25 * ljointv[j] + (((double)i + 1) * 0.25) * (((double)i + 1) * 0.25) * ljointa[j] + (((double)i + 1) * 0.25) * (((double)i + 1) * 0.25) * (((double)i + 1) * 0.25) * lastk[j];

                            fsMoveFollow << dTcp2Canbus4Points[i][j] << ", ";
                        }
                        fsMoveFollow << "\n";
                    }
                    //out.close();
                    for (int i = 0; i < 6; i++)
                    {
                        /*last_jointV[i] = (njoint[i] - last_joint[i]) / 6;*/
                        dLastJoint[i] = njoint[i];
                        ljointv[i] = 0.25 * ljointv[i] + 2.0 * 0.25 * ljointa[i] + 3.0 * 0.25 * lastk[i];
                        dLastJointV[i] = ljointv[i];
                        ljointa[i] = 2.0 * 0.25 * 0.25 * ljointa[i] + 3.0 * 2.0 * 0.25 * 0.25 * lastk[i];
                        dLastJointA[i] = ljointa[i];
                    }
                    ljoint = njoint;

                    //win
                    //rs_set_waypoints_to_canbus(rshdHand, dTcp2Canbus4Points, 4);
                    //linux
                    for(int i=0;i<4;i++)
                    {
                        robotService.robotServiceSetRobotPosData2Canbus(dTcp2Canbus4Points[i]);
                    }
                }
            }
        }
        else
        {
            //win
            //        JointVelcAccParam joint_maxacc = { 0 };
            //		JointVelcAccParam joint_maxvel = { 0 };
            //		for (int i = 0; i < 6; i++) {
            //			joint_maxacc.jointPara[i] = 0.5;
            //			joint_maxvel.jointPara[i] = 0.5;
            //		}
            //      rs_set_global_joint_maxacc(rshdHand, &joint_maxacc);
            //      rs_set_global_joint_maxvelc(rshdHand, &joint_maxvel);

            //linux
            aubo_robot_namespace::JointVelcAccParam joint_maxacc = { 0 };
            aubo_robot_namespace::JointVelcAccParam joint_maxvel = { 0 };
            for (int i = 0; i < 6; i++) {
                joint_maxacc.jointPara[i] = 0.5;
                joint_maxvel.jointPara[i] = 0.5;
            }
            robotService.robotServiceSetGlobalMoveJointMaxAcc(joint_maxacc);
            robotService.robotServiceSetGlobalMoveJointMaxVelc(joint_maxvel);

            //JointVelcAccParam joint_maxacc = { 0 };
            //JointVelcAccParam joint_maxvel = { 0 };
            aubo_robot_namespace::JointStatus jointStatus[6] = { 0 };
            // 		Eigen::Matrix3d robot_orimatrix;
            // 		Eigen::Vector3d robot_transmat;
                    /*	for (int i = 0; i < 6; i++) {
                            joint_maxacc.jointPara[i] = 0.5;
                            joint_maxvel.jointPara[i] = 0.5;
                        }*/
                        /*	rs_set_global_joint_maxacc(rshd, &joint_maxacc);
                            rs_set_global_joint_maxvelc(rshd, &joint_maxvel);*/
            //rs_leave_tcp2canbus_mode(rshdHand);
            //robotService.robotServiceLeaveTcp2CanbusMode();

            //开始就退出透传
            robotGetJoint(dLastJoint);
            //robot_forward(rshd, current_joint, robot_orimatrix, robot_transmat);
            //      robot_startMat.block(0, 0, 3, 3) = robot_orimatrix;
            //      robot_startMat.block(0, 3, 3, 1) = robot_transmat;

            //rs_enter_tcp2canbus_mode(rshdHand);
            robotService.robotServiceEnterTcp2CanbusMode();

            //std::ofstream out("C:/print1.txt", ios::app);
            for (int i = 0; i < 4; i++) {
                for (int j = 0; j < 6; j++)
                {
                    dTcp2Canbus4Points[i][j] = dLastJoint[j];

                    //out << tcp2canbus_6points[i][j] << " ";
                    if (i >= 3)
                    {
                        dLastJointV[j] = 0;
                        dLastJointA[j] = 0;
                    }

                }
                //out << endl;
            }
            //out.close();

            //win
            //rs_set_waypoints_to_canbus(rshdHand, dTcp2Canbus4Points, 4);
            //linux
            for(int i=0;i<4;i++)
            {
                robotService.robotServiceSetRobotPosData2Canbus(dTcp2Canbus4Points[i]);
            }


            //try other way
            //这里用高精度定时器模拟实时补偿微分速度？
            //QueryPerformanceCounter(&lilasttime);

            //Tips
            //QueryPerformanceCounter供Windows 95及其后续版本使用的精确时间函数

            bFollowStart = true;
        }

        return false;
}
/**
 * \brief   机械臂运动到目标位姿.
 *
 * \param desired_EndMatrix  目标位姿
 */
void AuboRobotControl::robotMove(Eigen::Matrix4d desired_EndMatrix)
{
	double target_postion[6];
	robotInverse(desired_EndMatrix, target_postion);
	robotMoveFollow(target_postion);

	return;
}

/**
 * \brief   获取机械臂当前状态.
 *
 * \return true 表示成功，false表示失败
 */
bool AuboRobotControl::robotGetConnectState()
{
    aubo_robot_namespace::RobotState status;
    //win
    //if (rs_get_robot_state(rshdHand, &status) == RS_SUCC)
    //linux
    if (robotService.robotServiceGetRobotCurrentState(status)  == aubo_robot_namespace::InterfaceCallSuccCode)
    {
        return true;
    }
    else
    {
        return false;
    }
}

/**
 * \brief   获取机械臂运行时状态数据.
 *
 * \param sRunTimedata  机械臂运行状态结构体变量应用
 */
void AuboRobotControl::GetRealtimeState(SRobotRealTimeData& sRunTimedata)
{
	GetRobotJointStatus(sRunTimedata);

	return;
}


/**
 * \brief   获取机械臂关节状态信息.
 *
 * \param sRunTimedata  机械臂运行状态信息结构体变量引用
 */
void AuboRobotControl::GetRobotJointStatus(SRobotRealTimeData& sRunTimedata)
{
	//机械臂各个关节角
    aubo_robot_namespace::JointStatus jointStatus[6] = { 0 };
	//机械臂末端姿态
	Eigen::Matrix3d rotationMatrix;
	//机械臂末端位置
	Eigen::Vector3d positionVector;
	robotGetJointStatus(jointStatus);
	for (int i = 0; i < 6; i++)
	{
		sRunTimedata.sRobotDy.dCoo6FJoint[i] = (double)jointStatus[i].jointPosJ;
	}

	robotForward(sRunTimedata.sRobotDy.dCoo6FJoint, rotationMatrix, positionVector);
	sRunTimedata.sRobotDy.m4dRobotEnd.block(0, 0, 3, 3) = rotationMatrix;
	sRunTimedata.sRobotDy.m4dRobotEnd.block(0, 3, 3, 1) = positionVector;

	sRunTimedata.sRobotDy.orientation = sRunTimedata.sRobotDy.m4dRobotEnd.block(0, 0, 3, 3);
	sRunTimedata.sRobotDy.position = sRunTimedata.sRobotDy.m4dRobotEnd.block(0, 3, 3, 1);

	return;
}
/**
 * \brief   获取机械臂当前状态信息.
 *
 * \param jointStatus      六个关节状态，包括：电流，电压，温度
 * \return                 true表示成功，false表示失败
 */
bool AuboRobotControl::robotGetJointStatus(aubo_robot_namespace::JointStatus* jointStatus)
{
    //linux
    if (robotService.robotServiceGetRobotJointStatus(jointStatus,6) == aubo_robot_namespace::InterfaceCallSuccCode)
    {
        return true;
    }
    else
    {
        return false;
    }
}


/**
 * \brief   通过输入机械臂末端的位姿，逆解出的路点信息.
 *
 * \param current_joint   机械臂关节角
 * \param pos             目标路点的位置 单位:米
 *\param ori              目标路点的参考姿态
 * \param waypoint        逆解出的路点信息
 * \return                true表示成功，false表示失败
 */
bool AuboRobotControl::robotInverse(double* current_joint, aubo_robot_namespace::Pos* pos, aubo_robot_namespace::Ori* ori, aubo_robot_namespace::wayPoint_S* waypoint)
{
    //linux(参考写法)
    //    int robotServiceRobotIk(const double *startPointJointAngle,const aubo_robot_namespace::Pos &position, const aubo_robot_namespace::Ori &ori, aubo_robot_namespace::wayPoint_S &wayPoint);
    //    int robotServiceRobotIk(const aubo_robot_namespace::Pos &position, const aubo_robot_namespace::Ori &ori, std::vector<aubo_robot_namespace::wayPoint_S> &wayPointVector);
    aubo_robot_namespace::Pos inputPos;
    aubo_robot_namespace::Ori inputOri;
    aubo_robot_namespace::wayPoint_S OutputJointPos;

    inputPos.x=pos->x;
    inputPos.y=pos->y;
    inputPos.z=pos->z;

    inputOri.w=ori->w;
    inputOri.x=ori->x;
    inputOri.y=ori->y;
    inputOri.z=ori->z;

    (robotService.robotServiceRobotIk(current_joint, inputPos, inputOri, OutputJointPos));
	return true;
}

/**
 * @brief          根据机械臂位姿，逆解出各个关节角
 * @param trans_matrix          机械臂位姿
 * @param joint_target          各个关节角
*/

void AuboRobotControl::robotInverse(Eigen::Matrix4d trans_matrix, double* joint_target)
{

	//LARGE_INTEGER  liBegintime, liEndtime, lilasttime;
	//LARGE_INTEGER liQpfrequency;
	//QueryPerformanceCounter(&lilasttime);
	//QueryPerformanceFrequency(&liQpfrequency);
	Eigen::Matrix3d orientation_matrix;
	Eigen::Vector3d transposition;
	orientation_matrix = trans_matrix.block(0, 0, 3, 3);
	transposition = trans_matrix.block(0, 3, 3, 1);
    //win
    // Pos position = { 0 };
    // Ori orientation = { 0 };
    // wayPoint_S joint_output = { 0 };

    //linux
    aubo_robot_namespace::Pos position = { 0,0,0 };
    aubo_robot_namespace::Ori orientation = { 0,0,0 };
    aubo_robot_namespace::wayPoint_S joint_output = { 0,0,0,0 };
	Eigen::Quaterniond quaternion(orientation_matrix);
	double current_joint[6];
	position.x = transposition(0);
	position.y = transposition(1);
	position.z = transposition(2);
	orientation.x = quaternion.x();
	orientation.y = quaternion.y();
	orientation.z = quaternion.z();
	orientation.w = quaternion.w();
	robotGetJoint(current_joint);
    //linux
    if (robotService.robotServiceRobotIk(current_joint, position, orientation, joint_output) == aubo_robot_namespace::InterfaceCallSuccCode)
	{
		for (int i = 0; i < 6; i++)
		{
			joint_target[i] = joint_output.jointpos[i];
		}
	}

	return;
}

/**
 * \brief   移除路点.
 *
 * \return  true表示成功，false表示失败
 */
bool AuboRobotControl::robotRemove()
{
    //win
    //int rs_remove_all_waypoint(rshdHand);

    //linux
    robotService.robotServiceClearGlobalWayPointVector();

	return true;
}

/**
 * \brief   获取机械臂当前关机角（jointangle）.
 *
 * \param rshd         械臂控制上下文句柄
 * \param jointangle   获取机械臂当前关机角（jointangle）
 * \return             true表示成功，false表示失败
 */
bool  AuboRobotControl::robotGetJoint(double* jointangle)
{
    aubo_robot_namespace::JointStatus jointStatus[6] = { 0 };
    //linux
    if (robotService.robotServiceGetRobotJointStatus(jointStatus, 6) == aubo_robot_namespace::InterfaceCallSuccCode) {

        for (int i = 0; i < 6; i++) {
            jointangle[i] = (double)jointStatus[i].jointPosJ;
        }
        return true;
    }
    else {
        return false;
    }
}

/**
 * \brief   通过输入机械臂各个关节角（jointangle），获取机械臂当前末端位姿（robot_ori，robot_trans）.
 *
 * \param jointangle   入机械臂各个关节角
 * \param robot_ori    获取机械臂当前末端位姿
 * \param robot_trans
 */
void  AuboRobotControl::robotForward(double jointangle[6], Eigen::Matrix3d& robot_ori, Eigen::Vector3d& robot_trans)
{
	aubo_robot_namespace::wayPoint_S wayPoint = { 0 };
	Eigen::Quaterniond qutern_mat;

    //linux
    if (robotService.robotServiceRobotFk(jointangle, aubo_robot_namespace::ARM_DOF, wayPoint) == aubo_robot_namespace::InterfaceCallSuccCode) {
		qutern_mat.x() = wayPoint.orientation.x;
		qutern_mat.y() = wayPoint.orientation.y;
		qutern_mat.z() = wayPoint.orientation.z;
		qutern_mat.w() = wayPoint.orientation.w;
		robot_ori = qutern_mat.toRotationMatrix();

		robot_trans(0) = wayPoint.cartPos.positionVector[0];
		robot_trans(1) = wayPoint.cartPos.positionVector[1];
		robot_trans(2) = wayPoint.cartPos.positionVector[2];
	}

	return;
}

void AuboRobotControl::test_hahaha()
{
	std::cout << "test haha" << std::endl;

	return;
}
